#pragma once

#include "MTMath.h"

namespace matsu
{
	template <typename T>
	inline Quaternion<T>::Quaternion() : x(0), y(0), z(0), w(1)
	{
	}

	template <typename T>
	inline Quaternion<T>::Quaternion(T x, T y, T z, T w) : x(x), y(y), z(z), w(w)
	{
	}

	// Ken Shoemake's famous method.
	template <typename T>
	inline Quaternion<T> Quaternion<T>::slerp(T t, const Quaternion<T>& v1) const
	{
		const T epsilon = 0.0005f;
		T dot = Dot(v1);

		if (dot > 1 - epsilon) {
			Quaternion<T> result = v1 + (*this - v1).Scaled(t);
			result.Normalize();
			return result;
		}

		if (dot < 0)
			dot = 0;

		if (dot > 1)
			dot = 1;

		T theta0 = std::acos(dot);
		T theta = theta0 * t;

		Quaternion<T> v2 = (v1 - Scaled(dot));
		v2.Normalize();

		Quaternion<T> q = Scaled(std::cos(theta)) + v2.Scaled(std::sin(theta));
		q.Normalize();
		return q;
	}

	template <typename T>
	inline Quaternion<T> Quaternion<T>::rotated(const Quaternion<T>& b) const
	{
		Quaternion<T> q;
		q.w = w * b.w - x * b.x - y * b.y - z * b.z;
		q.x = w * b.x + x * b.w + y * b.z - z * b.y;
		q.y = w * b.y + y * b.w + z * b.x - x * b.z;
		q.z = w * b.z + z * b.w + x * b.y - y * b.x;
		q.Normalize();
		return q;
	}

	template <typename T>
	inline Quaternion<T> Quaternion<T>::scaled(T s) const
	{
		return Quaternion<T>(x * s, y * s, z * s, w * s);
	}

	template <typename T>
	inline T Quaternion<T>::dot(const Quaternion<T>& q) const
	{
		return x * q.x + y * q.y + z * q.z + w * q.w;
	}

	template <typename T>
	inline Matrix3<T> Quaternion<T>::toMatrix() const
	{
		const T s = 2;
		T xs, ys, zs;
		T wx, wy, wz;
		T xx, xy, xz;
		T yy, yz, zz;
		xs = x * s;  ys = y * s;  zs = z * s;
		wx = w * xs; wy = w * ys; wz = w * zs;
		xx = x * xs; xy = x * ys; xz = x * zs;
		yy = y * ys; yz = y * zs; zz = z * zs;
		Matrix3<T> m;
		m.x.x = 1 - (yy + zz); m.y.x = xy - wz;  m.z.x = xz + wy;
		m.x.y = xy + wz; m.y.y = 1 - (xx + zz); m.z.y = yz - wx;
		m.x.z = xz - wy; m.y.z = yz + wx;  m.z.z = 1 - (xx + yy);
		return m;
	}

	template <typename T>
	inline Vector4<T> Quaternion<T>::toVector() const
	{
		return Vector4<T>(x, y, z, w);
	}

	template <typename T>
	Quaternion<T> Quaternion<T>::operator-(const Quaternion<T>& q) const
	{
		return Quaternion<T>(x - q.x, y - q.y, z - q.z, w - q.w);
	}

	template <typename T>
	Quaternion<T> Quaternion<T>::operator+(const Quaternion<T>& q) const
	{
		return Quaternion<T>(x + q.x, y + q.y, z + q.z, w + q.w);
	}

	template <typename T>
	bool Quaternion<T>::operator==(const Quaternion<T>& q) const
	{
		return x == q.x && y == q.y && z == q.z && w == q.w;
	}

	template <typename T>
	bool Quaternion<T>::operator!=(const Quaternion<T>& q) const
	{
		return !(*this == q);
	}

	// Compute the quaternion that rotates from a to b, avoiding numerical instability.
	// Taken from "The Shortest Arc Quaternion" by Stan Melax in "Game Programming Gems".
	template <typename T>
	inline Quaternion<T> Quaternion<T>::createFromVectors(const Vector3<T>& v0, const Vector3<T>& v1)
	{
		if (v0 == -v1)
			return Quaternion<T>::createFromAxisAngle(vec3(1, 0, 0), M_PI);

		Vector3<T> c = v0.cross(v1);
		T d = v0.dot(v1);
		T s = std::sqrt((1 + d) * 2);

		Quaternion<T> q;
		q.x = c.x / s;
		q.y = c.y / s;
		q.z = c.z / s;
		q.w = s / 2.0f;
		return q;
	}

	template <typename T>
	inline Quaternion<T>  Quaternion<T>::createFromAxisAngle(const Vector3<T>& axis, float radians)
	{
		Quaternion<T> q;
		q.w = std::cos(radians / 2);
		q.x = q.y = q.z = std::sin(radians / 2);
		q.x *= axis.x;
		q.y *= axis.y;
		q.z *= axis.z;
		return q;
	}

	template <typename T>
	inline void Quaternion<T>::normalize()
	{
		*this = Scaled(1 / std::sqrt(Dot(*this)));
	}

	template <typename T>
	inline void Quaternion<T>::rotate(const Quaternion<T>& q2)
	{
		Quaternion<T> q;
		Quaternion<T>& q1 = *this;

		q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
		q.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
		q.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
		q.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;

		q.normalize();
		*this = q;
	}
}
